#include "MotionModel.h"

using namespace std;

MotionModel::MotionModel() :
	config("config/general.cfg") {
}

MotionModel::~MotionModel() {
}

void MotionModel::stepRobot(PoseArray &paNow, const OdomMove &om, const Robot &robot,
		PoseArray &paNext, MATRIX &map) {
	//ROS_INFO("Bot stepping");
	double alpha = atof((robot.getConfiguration("robot.noise.alpha")).c_str());
	double beta = atof((robot.getConfiguration("robot.noise.beta")).c_str());
	double gamma = atof((robot.getConfiguration("robot.noise.gamma")).c_str());
	double delta = atof((robot.getConfiguration("robot.noise.delta")).c_str());

	double finalX, finalY, finalTheta, initialX, initialY, initialTheta, sampleX, sampleY,
			sampleTheta, rot1, rot2, trans, rot1Noise, rot2Noise, transNoise;

	paNext = paNow;
	for (int i = 0; i < paNext.size(); i++) {
		paNext[i].getPoseInOdom(initialX, initialY, initialTheta);

		double dtheta = piminuspiAngle(om.dtheta);
		finalX = initialX + om.dx;
		finalY = initialY + om.dy;
		finalTheta = initialTheta + dtheta;

		//		std::cout << om.dx << " " << om.dy << " " << " " << dtheta << "\n";

		rot1 = angleDiff(atan2(finalY - initialY, finalX - initialX), initialTheta);
		trans = sqrt((finalX - initialX) * (finalX - initialX) + (finalY - initialY) * (finalY
				- initialY));
		rot2 = finalTheta - initialTheta;

		assert(rot2 > -PI && rot2 < PI);

		transNoise = SampleNormal(trans, alpha * (trans + 0.001));
		rot1Noise = SampleNormal(rot1, beta);
		rot2Noise = SampleNormal(rot2, fabs(gamma * rot2) + fabs(delta * trans));

		sampleX = initialX + transNoise * cos(initialTheta + rot1Noise);
		sampleY = initialY + transNoise * sin(initialTheta + rot1Noise);
		double angleNoise = rot2Noise + initialTheta - finalTheta;

		if (abs(angleNoise) > 1 || abs(dtheta) > 1) {
			ROS_ERROR("%lf %lf %lf %lf %lf %lf %lf", rot1, rot2, rot1Noise, rot2Noise,
					initialTheta, finalTheta, angleNoise);
		}
		sampleTheta = finalTheta + angleNoise;//initialTheta + rot1Noise + rot2Noise;
//		sampleTheta = initialTheta;
		paNext[i].updatePoseInOdom(sampleX, sampleY, sampleTheta);
	}
	if (paNext.size() < paNow.size()) {
		ROS_ERROR("Step : Too close to the wall or in the netherworld. Not good X-(");
	}
}
/**
 *	Step function implements the motion model. It samples X(t) given X(t-1) and the control command.
 *	All operations in the step method are made in the odom frame.
 **/
void MotionModel::stepRobot2(PoseArray &paNow, const OdomMove &om, const Robot &robot,
		PoseArray &paNext, MATRIX &map) {
	double alpha1 = atof((robot.getConfiguration("robot.noise.alpha1")).c_str());
	double alpha2 = atof((robot.getConfiguration("robot.noise.alpha2")).c_str());
	double alpha3 = atof((robot.getConfiguration("robot.noise.alpha3")).c_str());
	double alpha4 = atof((robot.getConfiguration("robot.noise.alpha4")).c_str());

	double finalX, finalY, finalTheta, initialX, initialY, initialTheta, sampleX, sampleY,
			sampleTheta;
	double rot1, rot2, trans, rot1Noise, rot2Noise, transNoise;

	paNext = paNow;
	for (int i = 0; i < paNext.size(); i++) {
		paNext[i].getPoseInOdom(initialX, initialY, initialTheta);
		finalX = initialX + om.dx;
		finalY = initialY + om.dy;
		finalTheta = initialTheta + om.dtheta;

		/* Calculate change based on the odom readings */
		rot1 = angleDiff(atan2(finalY - initialY, finalX - initialX), initialTheta);
		trans = sqrt((finalX - initialX) * (finalX - initialX) + (finalY - initialY) * (finalY
				- initialY));
		rot2 = angleDiff(finalTheta - initialTheta, rot1);

		/* Sample noise along each dimension independently. Normalize the angles first */
		rot1Noise = std::min(abs(angleDiff(rot1, 0.0)), abs(angleDiff(rot1, PI)));
		rot2Noise = std::min(abs(angleDiff(rot2, 0.0)), abs(angleDiff(rot2, PI)));

		rot1Noise = SampleNormal(0, abs(/**(alpha1*rot1Noise)+**/(alpha2 * trans)));
		transNoise = SampleNormal(0, abs((alpha3 * trans) + /**(alpha4*rot1Noise)+**/(alpha4
				* rot2Noise)));
		rot2Noise = SampleNormal(0, abs((alpha1 * rot2Noise) + (alpha2 * trans)));

		/* Add Noise */
		rot1Noise = rot1 - rot1Noise;
		rot2Noise = rot2 - rot2Noise;
		transNoise = trans - transNoise;

		sampleX = initialX + transNoise * cos(initialTheta + rot1Noise);
		sampleY = initialY + transNoise * sin(initialTheta + rot1Noise);
		double angleNoise = rot1Noise + rot2Noise + initialTheta - finalTheta;
		sampleTheta = finalTheta + angleNoise;//initialTheta + rot1Noise + rot2Noise;
		if (sampleTheta - initialTheta > 2) {
			ROS_ERROR("%lf %lf %lf %lf %lf %lf %lf", rot1, rot2, rot1Noise, rot2Noise,
					initialTheta, finalTheta, angleNoise);
		}
		paNext[i].updatePoseInOdom(sampleX, sampleY, sampleTheta);
	}
	if (paNext.size() < paNow.size()) {
		ROS_ERROR("Step : Too close to the wall or in the netherworld. Not good X-(");
	}
}

/**
 * This is a custom version ofBrownian motion. Particles make constant sized steps, but the angles are not
 * chosen in a uniform distribution. Particles tend to remain in the same pose as they were before,
 * so the change in the pose is sampled normally with mean at 0, and variance sigma at PI.
 *
 * So at each step, the particle takes a small step and then moves in that direction for a constant sized step
 */
void MotionModel::stepHuman(PoseArray &paNow, PoseArray &paNext, MATRIX &map) {
	double radius = atof((config.getConfiguration("human.brownianRadius")).c_str()),
			radiusVariance = atof((config.getConfiguration("human.brownianRadiusVariance")).c_str()),
			thetaVariance = atof((config.getConfiguration("human.brownianThetaVariance")).c_str()),
			finalX, finalY, finalTheta, initialX, initialY, initialTheta, deltaTheta, deltaRadius;

	MyPose sample;
	paNext.clear();
	for (int i = 0, realitr = 0; i < paNow.size(); i++, realitr++) {
		sample = paNow[i];
		sample.getPoseInOdom(initialX, initialY, initialTheta);
		deltaTheta = SampleNormal(0, thetaVariance);
		finalTheta = initialTheta + deltaTheta;
		deltaRadius = SampleNormal(radius, radiusVariance);
		finalX = initialX + cos(finalTheta) * deltaRadius;
		finalY = initialY + sin(finalTheta) * deltaRadius;
		sample.updatePoseInOdom(finalX, finalY, finalTheta);
		if (!validParticle(sample, map) && realitr < paNow.size() + 1000)
			i--;
		else
			paNext.push_back(sample);
	}
	assert(paNext.size() == paNow.size());
}

bool MotionModel::validParticle(MyPose &pose, MATRIX &map) {
	double mapX, mapY, mapTheta;
	int mapIndexX, mapIndexY;
	pose.getPoseInMap(mapX, mapY, mapTheta);
	PII pos = index(mapX, mapY, CELL_RESOLUTION);
	mapIndexX = pos.first;
	mapIndexY = pos.second;

	/*Pose has shot out of the map*/
	if (mapIndexX >= map.size() || mapIndexY >= map[0].size())
		return false;

	/*Pose is in visitable space or hit on a wall*/
	if (map[mapIndexX][mapIndexY] == SPACE || map[mapIndexX][mapIndexY] == WALL)
		return true;

	/*Pose is out the visitable area of the map*/
	return false;
}
